Example: Planar System with Control and State Constraints
MEM 733
Professor Harry Kwatny

Setup

(* StateEquations={D[x1[t],t]==u1[t],D[x2[t],t]==u2[t]}; *)
J=(∫_0^(Subscript[t, 2]))1\[DifferentialD]t

ControlConstraint = (u1^2+u2^2-1)≤0;
StateConstraint = (x1-2)^2+x2^2-1≥0;

Hamiltonian and Adjoint Equations

H=1+λ1 u1+λ2 u2;

AdjointEquations={D[(Subscript[λ, 1])[t],t]==0,D[(Subscript[λ, 2])[t],t]==0};

Optimal Control

HH=H+k (u1^2+u2^2-1)

D[HH,u1]

D[HH,u2]

D[HH,k]

sols=FullSimplify[Solve[{D[HH,u1]==0,D[HH,u2]==0,D[HH,k]==0},{u1,u2,k}]]

FullSimplify[HH/.sols[[1]]]

FullSimplify[HH/.sols[[2]]]

Free Terminal Time Condition H=0

FullSimplify[H/.sols[[1]]]

FullSimplify[H/.sols[[2]]]

Reduce[FullSimplify[H/.sols[[2]]]==0,{λ1,λ2},Reals]

So (Subscript[λ, 1]),(Subscript[λ, 2]) lie on unit circle

Optimal Motion on Boundary

φ=1-(x1-2)^2-x2^2

c=FullSimplify[{D[φ,x1],D[φ,x2]}.{u1,u2},Reals]

HH=H+k (u1^2+u2^2-1)+h c

Parameterize obstacle boundary

c=FullSimplify[{D[φ,x1],D[φ,x2]}.{u1,u2},Reals]
HH=H+β (u1^2+u2^2-1)+α c
XRule={x1->2+Cos[θ],x2->Sin[θ]}

D[x1[t],t]=D[2+Cos[θ[t]],t]

sols=FullSimplify[Solve[{D[HH/.XRule,u1]==0,D[HH/.XRule,u2]==0,D[HH/.XRule,β]==0,D[HH/.XRule,α]==0}/.XRule,{u1,u2,α,β}]]

H/.sols[[1]]

t1=FullSimplify[Solve[(H/.sols[[1]])==0,λ2]]

FullSimplify[((2 α u1)/.sols[[1]])/.t1[[1]]]

Eqns=FullSimplify[D[λ1[θ],θ]==(-λ1[θ]+Sin[θ]) Tan[θ],Reals]

t2=FullSimplify[DSolve[Eqns,{λ1[θ]},θ]]

t3=FullSimplify[(t1[[1]]/.{λ2->λ2[θ],λ1->λ1[θ]})/.t2]

Boundary Conditions 1 - entry

BCEqns={Cos[(Subscript[ϕ, 0])]==λ1[θ]/.t2[[1]],-Sin[(Subscript[ϕ, 0])]==λ2[θ]/.t3[[1]]}

FullSimplify[Reduce[BCEqns,{C[1],(Subscript[ϕ, 0])},Reals]]